#!/usr/bin/env python
# -*- coding: UTF-8 -*-

import rospy

from cln_msgs.msg import CleanDeviceState,SensorState,HardwareCtrl,SensorType,CleanDeviceType,HardwareStateType,CleanDeviceMode
from cln_msgs.srv import *

from cln_msgs.msg import StringArray

from std_msgs.msg import Int8

# from tool import User_Data_Server
import time
from tool import Log_Parser
log = Log_Parser()

class Hardware_Parser:
    def __init__(self):
        self.__node_name = "Hardware_Parser"
        
        self.__robot_type = rospy.get_param("/robot_type_sw","SW55")    # 机器型号
        ## 数据服务
        # self.__data_server = User_Data_Server()

        self.__sensor_info = {} ##缓存传感器状态
        self.__cleandevice_state_msg = None
        
    def InitParser(self, node_name):
        '''
            初始化
        '''
        ## node name配置
        self.__node_name = node_name

        ##发布topic
        self.__sensor_ctrl_pub = rospy.Publisher("/sensor_ctrl", HardwareCtrl, queue_size=1)
        self.__cleandevice_ctrl_pub = rospy.Publisher("/cleandevice_ctrl", HardwareCtrl, queue_size=1)
        self.__automode_ctrl_pub = rospy.Publisher("/automode_ctrl", HardwareCtrl, latch=True, queue_size=1)
        self.__charge_ctrl_pub = rospy.Publisher("/charge_ctrl", HardwareCtrl, queue_size=1)
        self.__cleanmode_ctrl_pub = rospy.Publisher("/cleanmode_ctrl", CleanDeviceMode, queue_size=1)

        self.__robot_param_change = rospy.Publisher("/robot_param_change",StringArray,latch=True,queue_size=1)
        ### 接收到pad的外设控制模式数据，然后通过该话题发布出去，用于通知其他pad(mqtt订阅) 20230724-dw.liu
        self.__utils_ctrl_mode_pub = rospy.Publisher("/utils_ctrl_mode",Int8,latch=True,queue_size=1)
        
        ##订阅topic
        rospy.Subscriber("/sensors_state", SensorState, self.SensorsState_Callback, queue_size=1)
        rospy.Subscriber("/devices_state", CleanDeviceState, self.CleanDeviceState_Callback, queue_size=1)

    
    def GetNodeName(self):
        '''
            获取节点名称
        '''
        return self.__node_name
    #------------------------------

    #--------------------------------------#
    #******#  ROS topic publication #******# 
    #--------------------------------------#
    def PublishSensorControlTopic(self, cmd):
        '''
            传感器控制话题发布
        '''
        self.__sensor_ctrl_pub.publish(cmd)

    def PublishCleandeviceControlTopic(self, cmd):
        '''
            外设控制话题发布
        '''
        self.__cleandevice_ctrl_pub.publish(cmd)

    def PublishAutomodeControlTopic(self, cmd):
        '''
            手动控制模式话题发布
        '''
        self.__automode_ctrl_pub.publish(cmd)


    def PublishCleanmodeControlTopic(self, cmd):
        '''
            清扫控制模式话题发布
        '''
        self.__cleanmode_ctrl_pub.publish(cmd)

    def PublishChargeControlTopic(self, cmd):
        '''
            充电口控制话题发布
        '''
        self.__charge_ctrl_pub.publish(cmd)

    #-----------------------------------#
    #******#  ROS topic Callback #******# 
    #-----------------------------------#
    def SensorsState_Callback(self,msg_data):
        """ 
            传感器状态回调函数
        """
        # for sonar in msg_data.mul_sonar:
        #     self.UpdateSensorInfo(sonar)
        
        # for bumper in msg_data.mul_bumper:
        #     self.UpdateSensorInfo(bumper)

        # for drop in msg_data.mul_drop:
        #     self.UpdateSensorInfo(drop)

    def CleanDeviceState_Callback(self, msg):
        """ 
            外设状态回调函数
        """
        self.__cleandevice_state_msg = msg

    #-----------------------------------#
    #******#  intrinsic function #******# 
    #-----------------------------------#
    def UpdateSensorInfo(self, sensor):
        '''
            更新传感器信息
        '''
        # if self.__sensor_info.has_key(sensor.id.value):
        #     if sensor.online.state == sensor.online.ENABLE:
        #         self.__sensor_info[sensor.id.value]["enable"] = 1
        #     elif sensor.online.state == sensor.online.DISENABLE:
        #         self.__sensor_info[sensor.id.value]["enable"] = 0

    def CtrlBrushStop(self, brush_id, lift_id):
        '''
            控制主/边刷停止
            需要遵循以下流程：
            先停转，然后抬升
        '''
        #停转
        device_id_1 = CleanDeviceType()
        device_id_1.value = brush_id

        device_state_1 = HardwareStateType()
        device_state_1.state = device_state_1.OFF

        cmd_1 = HardwareCtrl()
        cmd_1.device_id.append(device_id_1)
        cmd_1.state.append(device_state_1)     
        self.PublishCleandeviceControlTopic(cmd_1) 
        
        #休眠 1s，等待停止
        # rospy.sleep(1.0) 
        time.sleep(1)

        #再抬升
        device_id_2 = CleanDeviceType()
        device_id_2.value = lift_id

        device_state_2 = HardwareStateType()
        device_state_2.state = device_state_2.UP

        cmd_2 = HardwareCtrl()
        cmd_2.device_id.append(device_id_2)
        cmd_2.state.append(device_state_2)     
        self.PublishCleandeviceControlTopic(cmd_2) 

    def CtrlBrushRotate(self, brush_id, lift_id, brush_state):
        '''
            控制主/边刷旋转
            需要遵循以下流程：
            先下降，然后旋转
        '''
        #下降
        device_id_1 = CleanDeviceType()
        device_id_1.value = lift_id

        device_state_1 = HardwareStateType()
        device_state_1.state = device_state_1.DOWM

        cmd_1 = HardwareCtrl()
        cmd_1.device_id.append(device_id_1)
        cmd_1.state.append(device_state_1)     
        self.PublishCleandeviceControlTopic(cmd_1) 
        
        #休眠 1s，等待停止
        # rospy.sleep(1.0) 
        time.sleep(1)

        #再旋转
        device_id_2 = CleanDeviceType()
        device_id_2.value = brush_id

        device_state_2 = HardwareStateType()
        device_state_2.state = brush_state

        cmd_2 = HardwareCtrl()
        cmd_2.device_id.append(device_id_2)
        cmd_2.state.append(device_state_2)     
        self.PublishCleandeviceControlTopic(cmd_2)

    def CtrlCarpetMode(self, carpet_mode_id, carpet_mode_state):
        '''
            控制地毯模式
        '''
        device_id = CleanDeviceType()
        device_id.value = carpet_mode_id

        device_state = HardwareStateType()
        device_state.state = carpet_mode_state

        cmd = HardwareCtrl()
        cmd.device_id.append(device_id)
        cmd.state.append(device_state)     
        self.PublishCleandeviceControlTopic(cmd)  

    def CtrlVacuumfang(self, vacuumfang_id, vacuumfang_state):
        '''
            控制真空风扇
            create-20230727-dw.liu
        '''
        device_id = CleanDeviceType()
        device_id.value = vacuumfang_id

        device_state = HardwareStateType()
        device_state.state = vacuumfang_state

        cmd = HardwareCtrl()
        cmd.device_id.append(device_id)
        cmd.state.append(device_state)    
        self.PublishCleandeviceControlTopic(cmd)
    
    def CtrlCarpetMode_Start(self):
        '''
            开启地毯模式
            create-20230727-dw.liu
        '''
        device_id = CleanDeviceType()
        device_id.value = 203

        device_state = HardwareStateType()
        device_state.state = 21

        cmd = HardwareCtrl()
        cmd.device_id.append(device_id)
        cmd.state.append(device_state)     
        self.PublishCleandeviceControlTopic(cmd) 

        SPEED_M = 22
        self.CtrlBrushRotate(CleanDeviceType().SIDEBRUSH_LF,CleanDeviceType().LIFT_LF,SPEED_M) # 左边刷 下降 中速
        self.CtrlBrushRotate(CleanDeviceType().SIDEBRUSH_RF,CleanDeviceType().LIFT_RF,SPEED_M) # 右边刷 下降 中速
        SPEED_M = 21
        self.CtrlBrushRotate(CleanDeviceType().MAIN_BRUSH,CleanDeviceType().LIFT_MAINBRUSH,SPEED_M) # 主刷 下降 低速 
        SPEED_M = 21
        self.CtrlVacuumfang(CleanDeviceType().VACUUMFAN, SPEED_M) # 真空风扇 低速

    #-----------------------------------#
    #******#  Interface function #******# 
    #-----------------------------------#
    def GetSensorList(self):
        '''
            获取传感器信息列表
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            # print(">>>>>>>>>>>>>> GetSensorList <<<<<<<<<<<<<")
            ret_dat["data"] = self.GetSensorInfo()
            
            
        except Exception as error:
            ret_dat["code"]=-1
            ret_dat["msg"] =str(error)

            err_msg = "[Hardware_Parser]Failed to get sensor information list: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
        return ret_dat 



    def GetSensorInfo(self):
        # 传感器信息初始化
        
        sensor_info_data = []
        # 语言 #0 中文 1 英文 2 繁体中文 3 西班牙语 4 法语 5 日语 6 韩语 7 德语   
        language_type = rospy.get_param("/language", 0)  
        language_dict = {0: "cn", 1: "en", 2: "tc", 3: "es", 4: "fr", 5: "ja", 6: "ko", 7: "de"}
        default_lang = "cn"
              
        # 深度相机传感器数量
        depth_camera_nums = rospy.get_param("/depth_camera_nums",0)
        
        # 获取深度相机配置参数
        depth_camera_params = rospy.get_param("/depth_camera_config", {})
        
        # 相机列表
        camera_lists = []
        if depth_camera_nums != 0:
            
            for camera_name, camera_param in depth_camera_params.items():
        
                # 相机列表
                camera_one = {}
                camera_one["used"] = 4
                
                camera_one["id"] = camera_param.get("frame_id",'camera_01_link')
                
                lang_key = language_dict.get(language_type, default_lang)
                camera_one["name"] = camera_param.get(lang_key, "Unknown")
                camera_one["name_ch"] = camera_one["name"]
                camera_one["enable"] = 0 if camera_param.get("disable", False) else 1
                
                camera_one["type"] = 0
                
                camera_lists.append(camera_one)
            
        # 固态雷达传感器数量
        solid_lidar_nums = rospy.get_param("/solid_lidar_nums",0)
        
        solid_lidar_list = []
        if solid_lidar_nums != 0:
            for i in range(1,solid_lidar_nums + 1):
                solid_lidar_prefix = "solid_lidar_0{}".format(i)
                solid_lidar_params = rospy.get_param("/solid_lidar_config",{})
                # 相机列表
                solid_lidar_one = {}
                solid_lidar_one["used"] = 4
                
                solid_lidar_one["id"] = solid_lidar_params.get(solid_lidar_prefix,{}).get("frame_id",'solid_lidar_01_link')
                
                lang_key = language_dict.get(language_type, default_lang)
                solid_lidar_one["name"] = solid_lidar_params.get(solid_lidar_prefix, {}).get(lang_key, "Unknown")
                solid_lidar_one["name_ch"] = solid_lidar_one["name"]
                solid_lidar_one["enable"] = 0 if solid_lidar_params.get(solid_lidar_prefix, {}).get("disable", False) else 1
                
                solid_lidar_one["type"] = 0
                
                solid_lidar_list.append(solid_lidar_one)
        
        # 超声波传感器数量
        sonar_lidar_list = []
        sonar_lidar_nums = rospy.get_param("/sonar_nums",0)
        if sonar_lidar_nums != 0:
            for i in range(1,sonar_lidar_nums + 1):
                sonar_lidar_prefix = "sonar_0{}".format(i)
                sonar_lidar_params = rospy.get_param("/sonar_config",{})
                # 相机列表
                sonar_lidar_one = {}
                sonar_lidar_one["used"] = 4
                
                sonar_lidar_one["id"] = sonar_lidar_params.get(sonar_lidar_prefix,{}).get("frame_id",'sonar_01_link')
                
                lang_key = language_dict.get(language_type, default_lang)
                sonar_lidar_one["name"] = sonar_lidar_params.get(sonar_lidar_prefix, {}).get(lang_key, "Unknown")
                sonar_lidar_one["name_ch"] = sonar_lidar_one["name"]
                sonar_lidar_one["enable"] = 0 if sonar_lidar_params.get(sonar_lidar_prefix, {}).get("disable", False) else 1
                
                sonar_lidar_one["type"] = 0
                
                sonar_lidar_list.append(sonar_lidar_one)
        
        # 激光雷达传感器数量
        lidar_2d_nums = rospy.get_param("/lidar_2d_nums",0)
        
        # IMU传感器数量
        sensor_info_data = camera_lists + solid_lidar_list + sonar_lidar_list
     
        return sensor_info_data 
        
        
    def Counter_tool(self, num):
        # 0 1 取反
        return 0 if num else 1

    def SetSensorEnabled(self,data):
        '''
            设置传感器使能检测
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data error!")
            
            sensor_id = data.get("id",'')
            if data.get("enable",0) == 0:
                enable = True
            else:
                enable = False
            self.SetSensorInfo(sensor_id,enable)
            
            
        except Exception as error:
            ret_dat["code"]=-1
            ret_dat["msg"] =str(error)

            err_msg = "[Hardware_Parser][SetSensorEnabled]Failed to set sensor enabled: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)

            rospy.logerr(err_msg)
        finally:
            return ret_dat 
        

    def SetSensorInfo(self,sensor_info_id,enable):
        print(">>>>>>>>>>>>>",sensor_info_id,enable)
        update_data = StringArray()
              
        # 深度相机传感器数量
        depth_camera_nums = rospy.get_param("/depth_camera_nums",2)
        
        # 获取深度相机配置参数
        depth_camera_params = rospy.get_param("/depth_camera_config", {})
        
        for camera_name, camera_param in depth_camera_params.items():
        
            print(camera_param.get("frame_id",''))
            if sensor_info_id == camera_param.get("frame_id",''):
                rospy.set_param("/depth_camera_config/{}/disable".format(camera_name), enable)
                update_data.data.append('/depth_camera_config')
        print(">>>>>>>>>>>>>>>>",update_data.data)
        
        if '/depth_camera_config' in update_data.data:
            # 通知其他节点参数变更
            self.__robot_param_change.publish(update_data)
            return
        
        # 固态雷达传感器数量
        solid_lidar_nums = rospy.get_param("/solid_lidar_nums",3)
          
        for i in range(1,solid_lidar_nums + 1):
            solid_lidar_prefix = "solid_lidar_0{}".format(i)
            solid_lidar_params = rospy.get_param("/solid_lidar_config",{})
            
            if sensor_info_id == solid_lidar_params.get(solid_lidar_prefix,{}).get("frame_id",''):
                rospy.set_param("/solid_lidar_config/{}/disable".format(solid_lidar_prefix), enable)
                update_data.data.append('/solid_lidar_config')
        print(">>>>>>>>>>>>>>>>",update_data.data)
        if '/solid_lidar_config' in update_data.data:
            # 通知其他节点参数变更
            self.__robot_param_change.publish(update_data)
            return
        
 
        # 超声波传感器数量
        sonar_lidar_nums = rospy.get_param("/sonar_nums",1)
          
        for i in range(1,sonar_lidar_nums + 1):
            sonar_lidar_prefix = "sonar_0{}".format(i)
            sonar_lidar_params = rospy.get_param("/sonar_config",{})
            
            if sensor_info_id == sonar_lidar_params.get(sonar_lidar_prefix,{}).get("frame_id",''):
                rospy.set_param("/sonar_config/{}/disable".format(sonar_lidar_prefix), enable)
                update_data.data.append('/sonar_config')
        print(">>>>>>>>>>>>>>>>",update_data.data)
        if '/sonar_config' in update_data.data:
            # 通知其他节点参数变更
            self.__robot_param_change.publish(update_data)
            return        

        
        # 激光雷达传感器数量
        lidar_2d_nums = rospy.get_param("/lidar_2d_nums",1)
        
        # IMU传感器数量
        

    def SetHandMode(self, data):
        '''
            设置手动模式
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data is empty!")
            mode = data["mode"]

            state = HardwareStateType()
            state.state = state.OFF if (1 == mode) else state.ON

            cmd = HardwareCtrl()
            cmd.state.append(state)     
            self.PublishAutomodeControlTopic(cmd)

            log_msg = "[Robot_Parser]Set hand mode to <{}>.".format(mode)
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to set hand mode: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            rospy.logerr(err_msg)
        finally:
            return ret_dat


    def SetChargeSwitch(self, data):
        '''
            设置充电开关
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data is empty!")
            mode = data["ctrl_mode"]

            state = HardwareStateType()
            state.state = state.ON if (1 == mode) else state.OFF

            cmd = HardwareCtrl()
            cmd.state.append(state)     
            self.PublishChargeControlTopic(cmd)
            
            rospy.set_param('/robot/electrode_state', mode)

            log_msg = "[Robot_Parser]Set charge switch to <{}>.".format(mode)
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to set charge switch: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            rospy.logerr(err_msg)
        finally:
            return ret_dat
        
    #---------------------------------#
    # ft.wang 20240606接手
    #---------------------------------#

    def GetDeviceList(self):
        '''
            获取外设列表
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        ret_dat["data"] = list()
        try:
            # 更新基础参数
            device_info_to_app_params = rospy.get_param("/device_info_to_app",{})
            for device_info in device_info_to_app_params:
                new_device_info = device_info.copy()
                # 更新外设状态值
                if self.__cleandevice_state_msg:
                    if device_info["id"] == CleanDeviceType.MAIN_BRUSH:
                        new_device_info["state"] = self.__cleandevice_state_msg.main_brush.state.state
                    if device_info["id"] == CleanDeviceType.SIDE_BRUSH:
                        new_device_info["state"] = self.__cleandevice_state_msg.side_brush.state.state
                    if device_info["id"] == CleanDeviceType.LIFT_MAINBRUSH:
                        new_device_info["state"] = self.__cleandevice_state_msg.lift_mainbrush.state.state
                    if device_info["id"] == CleanDeviceType.LIFT_SIDEBRUSH:
                        new_device_info["state"] = self.__cleandevice_state_msg.lift_sidebrush.state.state
                    if device_info["id"] == CleanDeviceType.CARPET_MODE:
                        new_device_info["state"] = self.__cleandevice_state_msg.carpet_mode.state.state
                    if device_info["id"] == CleanDeviceType.UV_LED:
                        new_device_info["state"] = self.__cleandevice_state_msg.uv_led.state.state
                ret_dat["data"].append(new_device_info)
            log_msg = "[Hardware_Parser]Succeed to get device list."
            log.LogInfo(self.GetNodeName(),log_msg)  
        except Exception as error:
            ret_dat["code"]=-1
            ret_dat["msg"] =str(error)
            err_msg = "[Hardware_Parser]Failed to get device list: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
        return ret_dat 

    def PublishDeviceState(self, value, state):
        device_id = CleanDeviceType()
        device_id.value = value
        device_state = HardwareStateType()
        device_state.state = state  
        cmd = HardwareCtrl()
        cmd.device_id.append(device_id)
        cmd.state.append(device_state)    
        self.PublishCleandeviceControlTopic(cmd)
        time.sleep(0.1)

    def SetDeviceState(self,data):
        '''
            外设设置
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data error!")
            id =  data["id"]
            state = data["value"]

            # 新增主刷高度自定义203控制时，先发送主刷低速，同时22置为关
            if int(id) == CleanDeviceType.CARPET_MODE:
                if int(state) > 0:
                    # 下发外设控制指令
                    self.PublishDeviceState(CleanDeviceType.MAIN_BRUSH, HardwareStateType.SPEED_L)
                self.PublishDeviceState(id, state)

            # 主刷高度升降控制时，203置为0
            elif int(id) == CleanDeviceType.LIFT_MAINBRUSH:
                self.PublishDeviceState(id, state)
                self.PublishDeviceState(CleanDeviceType.CARPET_MODE, 0)
            # 点主刷关时还需要发送关地毯模式的指令
            elif int(id) == CleanDeviceType.MAIN_BRUSH and int(state) == 8:
                self.PublishDeviceState(CleanDeviceType.CARPET_MODE, 0)
                self.PublishDeviceState(id, state)
            else:
                self.PublishDeviceState(id, state)

        
            log_msg = "[Hardware_Parser]Set device<ID: {}> state: {}.".format(id,state)
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"]=-1
            #ret_dat["ret"]='fail'
            ret_dat["msg"] =str(error)

            err_msg = "[Hardware_Parser]Failed to set disinfect speed: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)

            rospy.logerr(err_msg)
        finally:
            return ret_dat 

    
    def SetDevicesdMode(self, data):
        '''
            外设控制模式
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data is empty!")
            mode = data["mode"] # 0全部关闭 2全部开启 3自检  

            cmd = HardwareCtrl()
            device_id = CleanDeviceType()
            device_state = HardwareStateType()

            # 更新默认清扫模式参数 0全部关闭 mode_1静音模式 2全部开启 3自检 mode_4地毯模式 mode_2硬质模式  mode_* 用户自定义模式
            # print("mode == '0': ", mode == '0')
            # print("mode == 0: ", mode == 0)
            mode_name = None
            if mode == '0' or mode == 0:
                device_id.value = device_id.ALL_DEVICE
                device_state.state = device_state.OFF
            elif mode == '1'  or mode == 1 or mode == "mode_1":
                mode_name = "mode_1"    #静音模式
            elif mode == '2' or mode == 2:
                device_id.value = device_id.ALL_DEVICE
                device_state.state = device_state.ON
            elif mode == '3' or mode == 3:
                device_id.value = device_id.ALL_DEVICE_SELFCHECKING
                device_state.state = device_state.ON
            elif mode == '4' or mode == 4 or mode == "mode_4":
                mode_name = "mode_4"    #地毯模式
            elif mode == '5' or mode == 5 or mode == "mode_2":
                mode_name = "mode_2"    #硬质地面
            elif "mode_" in mode:
                mode_name = mode    #自定义用户模式
            else:
                mode_name = mode    #防止键值异常
            # print("mode_name: ",mode_name)
            # print("mode: ",mode)
            if mode_name:
                cleandevice_config_params = rospy.get_param("/cleandevice_mode_config",{})
                for key, value in cleandevice_config_params.items():
                    if key != 'map':
                        if key == mode_name:
                            cmd.user_defined_mode = str(value["item_name"])
                                   
            cmd.device_id.append(device_id)
            cmd.state.append(device_state) 
            self.PublishCleandeviceControlTopic(cmd)  
            
            # self.__utils_ctrl_mode_pub.publish(int(data["mode"])) # 20230724-dw.liu

            log_msg = "[Robot_Parser]Set device control mode to <{}>.".format(mode)
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to set devive control mode: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            rospy.logerr(err_msg)
        finally:
            return ret_dat